#include "./motors.h"
#include "../xlib/gpio.h"
#include "../rtx/rtx_os.h"
#include "./sensor.h"

namespace heat_driver {

Motors motors;

namespace motors_impl {

using namespace stm32f1::gpio;
using namespace stm32f1;

using DIR1 = PA<0>;
using DIR2 = PA<1>;
using STIR_CAP = PA<5>;
using HOLD = PA<6>;
using STIR = PA<7>;
using FILM = PA<8>;

enum class Dir : uint8_t {
    CW = 0,
    CCW = 1,
    BRAKE = 2,
};

static void set_dir(Dir dir) {

    switch (dir) {
        case Dir::CW: 
            DIR1::set();
            DIR2::clr();
        break;

        case Dir::CCW:
            DIR1::clr();
            DIR2::set();
        break;

        case Dir::BRAKE:
            DIR1::clr();
            DIR2::clr();
        break;
    }
}

}

using namespace motors_impl;

void Motors::init() {

    rcc::enable<
        DIR1, DIR2, STIR_CAP, HOLD, STIR
    >();

    config<
        Cfg<DIR1, Mode::OUT_50MHz_PP>,
        Cfg<DIR2, Mode::OUT_50MHz_PP>,
        Cfg<STIR_CAP, Mode::OUT_50MHz_PP>,
        Cfg<HOLD, Mode::OUT_50MHz_PP>,
        Cfg<STIR, Mode::OUT_50MHz_PP>,
        Cfg<FILM, Mode::OUT_50MHz_PP>
    >();

    DIR1::clr();
    DIR2::clr();
    STIR_CAP::clr();
    HOLD::clr();
    STIR::clr();
}

Error Motors::stir_cap_open() {
    Error err;
    uint32_t time_count = 0;
    uint32_t alm_count = 0;
    if (sensor->is_stir_top()) {
        goto finish;
    }

    set_dir(Dir::CW);
    STIR_CAP::set();
    for (;;) {
        if (sensor->is_stir_top()) {
            break;
        }

        if ((time_count >= 500) && (sensor.adc_value() >= STIR_CAP_ALM)) {
            alm_count ++;
            if (alm_count >= 25) {
                err = ec::heat::STIR_MOTOR_ALM;
                goto finish;
            }
        } else {
            alm_count = 0;
        }

        osDelay(20);
        time_count += 20;
    
        if (time_count >= STIR_CAP_TIMEOUT) {
            err = ec::heat::STIR_MOTOR_TIMEOUT;
            goto finish;
        }
    }
finish:
    set_dir(Dir::BRAKE);
    STIR_CAP::clr();
    return err;
}

Error Motors::stir_cap_close() {
    Error err;
    uint32_t time_count = 0;
    uint32_t alm_count = 0;
    if (sensor->is_stir_bottom()) {
        goto finish;
    }

    set_dir(Dir::CCW);
    STIR_CAP::set();
    for (;;) {
        if (sensor->is_stir_bottom()) {
            break;
        }

        if ((time_count >= 500) && (sensor.adc_value() >= STIR_CAP_ALM)) {
            alm_count ++;
            if (alm_count >= 25) {
                err = ec::heat::STIR_MOTOR_ALM;
                goto finish;
            }
        } else {
            alm_count = 0;
        }

        osDelay(20);
        time_count += 20;
    
        if (time_count >= STIR_CAP_TIMEOUT) {
            err = ec::heat::STIR_MOTOR_TIMEOUT;
            goto finish;
        }
    }
finish:
    set_dir(Dir::BRAKE);
    STIR_CAP::clr();
    return err;
}

Error Motors::hold_left() {
    Error err;
    uint32_t time_count = 0;
    uint32_t alm_count = 0;
    if (sensor->is_hold_left()) {
        goto finish;
    }

    set_dir(Dir::CCW);
    HOLD::set();
    for (;;) {
        if (sensor->is_hold_left()) {
            break;
        }

        if ((time_count >= 500) && (sensor.adc_value() >= HOLD_ALM)) {
            alm_count ++;
            if (alm_count >= 25) {
                err = ec::heat::HOLD_MOTOR_ALM;
                goto finish;
            }
        } else {
            alm_count = 0;
        }

        osDelay(20);
        time_count += 20;
    
        if (time_count >= HOLD_TIMEOUT) {
            err = ec::heat::HOLD_MOTOR_TIMEOUT;
            goto finish;
        }
    }
finish:
    set_dir(Dir::BRAKE);
    HOLD::clr();
    return err;
}

Error Motors::hold_right() {
    Error err;
    uint32_t time_count = 0;
    uint32_t alm_count = 0;

    if (sensor->is_hold_right()) {
        goto finish;
    }

    set_dir(Dir::CW);
    HOLD::set();
    for (;;) {
        if (sensor->is_hold_right()) {
            break;
        }

        if ((time_count >= 500) && (sensor.adc_value() >= HOLD_ALM)) {
            alm_count ++;
            if (alm_count >= 25) {
                err = ec::heat::HOLD_MOTOR_ALM;
                goto finish;
            }
        } else {
            alm_count = 0;
        }

        osDelay(20);
        time_count += 20;
    
        if (time_count >= HOLD_TIMEOUT) {
            err = ec::heat::HOLD_MOTOR_TIMEOUT;
            goto finish;
        }
    }
finish:
    set_dir(Dir::BRAKE);
    HOLD::clr();
    return err;
}

Error Motors::film_up() {
    Error err;
    uint32_t time_count = 0;
    uint32_t alm_count = 0;
    if (sensor->is_film_up()) {
        goto finish;
    }

    set_dir(Dir::CW);
    FILM::set();
    for (;;) {
        if (sensor->is_film_up()) {
            break;
        }

        if ((time_count >= 500) && (sensor.adc_value() >= FILM_ALM)) {
            alm_count ++;
            if (alm_count >= 25) {
                err = ec::heat::FILM_MOTOR_ALM;
                goto finish;
            }
        } else {
            alm_count = 0;
        }

        osDelay(20);
        time_count += 20;
    
        if (time_count >= FILM_TIMEOUT) {
            err = ec::heat::FILM_MOTOR_TIMEOUT;
            goto finish;
        }
    }
finish:
    set_dir(Dir::BRAKE);
    FILM::clr();
    return err;
}

Error Motors::film_down() {
    Error err;
    uint32_t time_count = 0;
    uint32_t alm_count = 0;
    if (sensor->is_film_down()) {
        goto finish;
    }

    set_dir(Dir::CCW);
    FILM::set();
    for (;;) {
        if (sensor->is_film_down()) {
            break;
        }

        if ((time_count >= 500) && (sensor.adc_value() >= FILM_ALM)) {
            alm_count ++;
            if (alm_count >= 25) {
                err = ec::heat::FILM_MOTOR_ALM;
                goto finish;
            }
        } else {
            alm_count = 0;
        }

        osDelay(20);
        time_count += 20;
    
        if (time_count >= FILM_TIMEOUT) {
            err = ec::heat::FILM_MOTOR_TIMEOUT;
            goto finish;
        }
    }
finish:
    set_dir(Dir::BRAKE);
    FILM::clr();
    return err;
}

void Motors::stir_open_cw() {
    set_dir(Dir::CW);
    STIR::set();
}

void Motors::stir_open_ccw() {
    set_dir(Dir::CCW);
    STIR::set();
}

void Motors::stir_close() {
    set_dir(Dir::BRAKE);
    STIR::clr();
}

}
